﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using System.ComponentModel;

namespace KinectAdapter
{
    /// <summary>
    /// This is a Kinect adapter that can be used to operate a robotic forklift.
    /// </summary>
    public abstract class KinectForkliftAdapter : IKinectState
    {
        public event PropertyChangedEventHandler PropertyChanged = null;
        public virtual event EventHandler StopIssued = null;
        public virtual event EventHandler MenuIssued = null;

        #region Fields and contants

        public const string ButlaStyle = "ButlaStyle";
        public const string AllInRightHand = "AllInRightHand";
        public const string SteeringWheel = "SteeringWheel";

        protected const double DEFAULT_DATA_SCALE = 1.0;
        protected const double DEFAULT_ZERO_RANGE = 0.15;
        protected const double DEFAULT_MIN = -1.0;
        protected const double DEFAULT_MAX = 1.0;
        /// <summary>
        /// The movement of the steering hand will be calculated
        /// using this much of its last known positions.
        /// Must be at least 2.
        /// </summary>
        protected const int MOVEMENT_POINTS = 3;

        /// <summary>
        /// If two joints are this far away, they're considered as touching.
        /// </summary>
        protected const double JOINT_TOUCH_RANGE = 0.15;

        [Obsolete]
        protected float handRightLastX = 0;
        [Obsolete]
        protected float handRightLastZ = 0;
        [Obsolete]
        protected float handRightLastY = 0;
        protected List<SkeletonPoint> handPositions = new List<SkeletonPoint>();

        [Obsolete]
        protected bool firstFrame = true;

        protected bool mainSteeringOn = false;
        protected bool secondarySteeringOn = false;

        private OperatorSkeleton operatorSkeleton = null;

        #endregion

        #region Properties

        public KinectSlider X { get; protected set; }
        public KinectSlider Y { get; protected set; }
        public KinectSlider Z { get; protected set; }

        public OperatorSkeleton OperatorSkeleton
        {
            get { return operatorSkeleton; }
            set { operatorSkeleton = value; }
        }

        /// <summary>
        /// Gets of sets the value that indicates if primary steering
        /// should be enabled.
        /// </summary>
        public bool MainSteeringOn
        {
            get { return mainSteeringOn; }
            set
            {
                if (mainSteeringOn != value)
                {
                    mainSteeringOn = value;
                    NotifyPropertyChanged("MainSteeringOn");
                }
            }
        }

        /// <summary>
        /// Gets of sets the value that indicates if secondary steering
        /// should be enabled.
        /// </summary>
        public bool SecondarySteeringOn
        {
            get { return secondarySteeringOn; }
            set
            {
                if (secondarySteeringOn != value)
                {
                    secondarySteeringOn = value;
                    NotifyPropertyChanged("SecondarySteeringOn");
                }
            }
        }

        /// <summary>
        /// Will be set to false when the application returns to menu.
        /// No gestures will be detected during that time.
        /// </summary>
        public bool IsActive { get; set; }

        #endregion

        #region Private methods

        private void NotifyPropertyChanged(String info)
        {
            if (PropertyChanged != null)
            {
                PropertyChanged(this, new PropertyChangedEventArgs(info));
            }
        }

        #endregion

        #region Public methods

        /// <summary>
        /// Initiates the adapter.
        /// Start() methods needs to be called for it to start working.
        /// </summary>
        public KinectForkliftAdapter()
        {
            IsActive = true;
            operatorSkeleton = new OperatorSkeleton();
            operatorSkeleton.SkeletonUpdated += GetSkeletonData;
            X = new KinectSlider() { Min = DEFAULT_MIN, Max = DEFAULT_MAX, Scale = DEFAULT_DATA_SCALE, ZeroRange = DEFAULT_ZERO_RANGE };
            Y = new KinectSlider() { Min = DEFAULT_MIN, Max = DEFAULT_MAX, Scale = DEFAULT_DATA_SCALE, ZeroRange = DEFAULT_ZERO_RANGE };
            Z = new KinectSlider() { Min = DEFAULT_MIN, Max = DEFAULT_MAX, Scale = DEFAULT_DATA_SCALE, ZeroRange = DEFAULT_ZERO_RANGE };
        }

        /// <summary>
        /// Starts the adapter. This turns on Kinect.
        /// </summary>
        public bool Start()
        {            
            return OperatorSkeleton.Start();
        }

        /// <summary>
        /// Stops the adapter. This turns off Kinect.
        /// </summary>
        public void Stop()
        {
            OperatorSkeleton.Stop();
        }

        /// <summary>
        /// Resets controls so that the forklift will stop.
        /// </summary>
        public void Halt()
        {
            X.Reset();
            Y.Reset();
            Z.Reset();
            MainSteeringOn = false;
            SecondarySteeringOn = false;
        }

        #endregion

        #region Protected methods

        /// <summary>
        /// This should handle the data received from Kinect regarding the operator's position.
        /// </summary>
        /// <param name="skeleton">Data about the operator's skeleton.</param>
        protected abstract void GetSkeletonData(Skeleton skeleton);

        /// <summary>
        /// This should check if the main steering should be enabled based on the operator's skeleton.
        /// </summary>
        /// <param name="skeleton">Operator's skeleton.</param>
        /// <returns>True if the main steering should be enabled.</returns>
        protected abstract bool CheckMainSteering(Skeleton skeleton);

        /// <summary>
        /// This should check if the secondary steering should be enabled based on the operator's skeleton.
        /// </summary>
        /// <param name="skeleton">Operator's skeleton.</param>
        /// <returns>True if the secondary steering should be enabled.</returns>
        protected abstract bool CheckSecondarySteering(Skeleton skeleton);

        /// <summary>
        /// This should check if the operator made a gesture indicating that all controls
        /// should be restored to their defaults (stop the forklift).
        /// </summary>
        /// <param name="skeleton">Operator's skeleton.</param>
        /// <returns>True if the forklift should stop.</returns>
        protected abstract bool CheckStop(Skeleton skeleton);

        /// <summary>
        /// This should check if the operator made a gesture indicating that application
        /// should return to menu. HUD will still be working but will be hidden.
        /// Also, the forklift should stop.
        /// </summary>
        /// <param name="skeleton">Operator's skeleton.</param>
        /// <returns>True if the application should return to menu.</returns>
        protected abstract bool CheckMenu(Skeleton skeleton);

        /// <summary>
        /// Updates the list of previous positions of the steering hand.
        /// </summary>
        /// <param name="skeleton"></param>
        protected void UpdateSteeringHandPositions(Skeleton skeleton)
        {
            // old way, left for the time of testing the new way
            handRightLastX = skeleton.Joints[JointType.HandRight].Position.X;
            handRightLastY = skeleton.Joints[JointType.HandRight].Position.Y;
            handRightLastZ = skeleton.Joints[JointType.HandRight].Position.Z;

            // new way, just the list, no handRightLastX etc
            handPositions.Add(skeleton.Joints[JointType.HandRight].Position);
            if (handPositions.Count > MOVEMENT_POINTS)  // if the list has now more than the specified number of elements we delete the oldest one
                handPositions.RemoveAt(0);
        }

        /// <summary>
        /// Gets the mean change of position in the axis, from which the numbers are taken.
        /// </summary>
        /// <param name="numbers">Values of one axis variable for the remembered frames.
        /// Use Select() and ToArray() on a list to get them.</param>
        /// <returns>Mean change of position along the picked axis for the remembered frames.
        /// Zero if the movement wasn't in one direction for all the frames
        /// taken into consideration.</returns>
        protected float GetMeanChange(float[] numbers)
        {
            bool oneDirection = true;
            float combinedChange = 0.0f;
            float oldCombinedChange = 0.0f;
            for (int i = 0; i < numbers.Length - 1; i++)
            {
                combinedChange += (numbers[i + 1] - numbers[i]);
                /*
                 * Checking if in all the frames the change is in one direction.
                 * If the absolute value will decrease after adding the change between two frames
                 * then a value with different sign than the others was added.
                 * The new value with a different sing can be so big that the combined absolute value will be bigger,
                 * so we check if the old and the new combined values have the same sign.
                 */
                if (Math.Abs(combinedChange) < Math.Abs(oldCombinedChange) ||
                    combinedChange * oldCombinedChange < 0)
                {
                    oneDirection = false;
                }

                oldCombinedChange = combinedChange;
            }

            if (oneDirection)
                return combinedChange / (float)(handPositions.Count - 1);   // mean
            else
                return 0.0f;
        }
        
        #endregion
    }
}
